import math
import time
import numpy as np
import NetFT
import Robot.UR5 as Rb
import Robot.FTsensor as FT
from pyquaternion import Quaternion
import scipy
import Robot.sim as sim_interface
from algorithm import planning as pl
from algorithm.mathlib import utilities as utl
from algorithm.mathlib.utilities import PoseRobot
import multiprocessing as mp
import time, random

from algorithm.control.admittance import Admittance


def connect(shared_in_data, shared_out_data, lock):
    # netft_ip = "192.168.1.1"
    # ft = NetFT.Sensor(netft_ip)
    # ft.setbias()
    sim_vrep = sim_interface.sim()

    for i in range(1):
        sim_vrep.sim_read_init_data()

    while True:
        sim_vrep.sim_read_data()
        with lock:
            shared_in_data.tip_position = sim_vrep.read_data.tip_position
            shared_in_data.tip_orientation = sim_vrep.read_data.tip_orientation
            shared_in_data.forceVector = sim_vrep.read_data.forceVector
            shared_in_data.torqueVector = sim_vrep.read_data.torqueVector
            pass

def task(shared_in_data, shared_out_data, robot, lock):


    while True:
        lock.acquire()
        robot.sim_read_data(shared_in_data)
        # shared_out_data = robot.sim_send_data()
        lock.release()

if __name__ == '__main__':
    lock = mp.Lock()
    shared_in_data = sim_interface.read_data_package()
    shared_out_data = sim_interface.send_data_package()

    ati = FT.FTSensor(Rb.lowpass_filter)
    admittance = Admittance()

    robot = Rb.RobotUR5()
    robot.ft_sensor = ati

    # sim_vrep.sim_read_data()
    # target: name of function
    connect_thrd = mp.Process(target=connect, args=(shared_in_data, shared_out_data,  lock))
    task_thrd = mp.Process(target=task, args=(shared_in_data, shared_out_data, robot, lock))

    connect_thrd.start()
    task_thrd.start()
    #
    connect_thrd.join()
    task_thrd.join()

